#include "livox_m.h"
#include <cstdio>
#include <cstring>
//#include <thread>
#include <memory>
/** Const varible ------------------------------------------------------------------------------- */
/** User add broadcast code here */
boost::mutex LdsLidar::change_mu;
boost::mutex LdsLidar::imu_mu;
boost::mutex LdsLidar::FrameLock;
std::shared_ptr<Eigen::Vector3d> LdsLidar::macc;
std::shared_ptr<Eigen::Vector3d> LdsLidar::mgyro;
// std::vector<LivoxEthPacket *> RawClientDatas;
Gydata LdsLidar::OriginRoT;
std::deque<CloudI> LdsLidar::TempAll;
PcloudI LdsLidar::ChangedCloudI(new CloudI());
PcloudI LdsLidar::Icloud;
bool LdsLidar::Tags[250];

// LdsLidar *this_ = nullptr;
// Eigen::Matrix4f LdsLidar::RoT;
// Eigen::Matrix4f LdsLidar::CamIntrix;
// cv::Mat LdsLidar::DepthImg;
static const char *local_broadcast_code_list[] = {
    "0TFDFG700601861",
    "3WEDH7600110891"};
/** For callback use only */
LdsLidar *g_lidars = nullptr;
// Gyodometry &myodo = Gyodometry::GetInstance();
/** Lds lidar function ---------------------------------------------------------------------------*/
// LdsLidar::LdsLidar() : RawDatas(48 * 500)
LdsLidar::LdsLidar()
{
    // for(size_t i = 0; i < 5; i++)
    // {
    //     TempAll[i].clear();
    //     TempAll[i].resize(96*250);
    //     // OutputAll[i]->resize(96*250);
    // }

    ChangedCloudI->points.resize(96 * 256);
    // myodo.DataCL();
    RoTs.resize(5);
    auto_connect_mode_ = true;
    whitelist_count_ = 0;
    is_initialized_ = false;
    lidar_count_ = 0;
    memset(broadcast_code_whitelist_, 0, sizeof(broadcast_code_whitelist_));
    memset(lidars_, 0, sizeof(lidars_));
    RoT << -0.0237806, -0.999379, 0.0259964, 0.183787,
        -0.0103828, -0.0257555, -0.999614, 0.020738,
        0.999663, -0.0240413, -0.00976387, -0.369433,
        0, 0, 0, 1;
    // CamIntrix << 1263.963, 0.0, 620.6840,
    //     0.0, 1263.660, 532.8492,
    //     0, 0, 1.0;
    CamIntrix << 1713.91138912005, 0.143349090450384, 627.578597951611,
        0, 1714.20429945400, 486.114880603001,
        0, 0, 1;

    // DepthImg = cv::Mat::zeros(1024, 1280, CV_8UC1);
    for (uint32_t i = 0; i < kMaxLidarCount; i++)
    {
        lidars_[i].handle = kMaxLidarCount;
        /** Unallocated state */
        lidars_[i].connect_state = kConnectStateOff;
    }
}

LdsLidar::~LdsLidar()
{
}

int LdsLidar::InitLdsLidar(std::vector<std::string> &broadcast_code_strs, int max_size)
{

    if (is_initialized_)
    {
        printf("LiDAR data source is already inited!\n");
        return -1;
    }
    if (!Init())
    {
        Uninit();
        printf("Livox-SDK init fail!\n");
        return -1;
    }
    max_cloud_size = max_size;
    cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
    cloud->points.resize(max_cloud_size, pcl::PointXYZ(0, 0, 0));
    Icloud.reset(new CloudI());
    Icloud->points.resize(max_cloud_size, pcl::PointXYZI(0));

    LivoxSdkVersion _sdkversion;
    GetLivoxSdkVersion(&_sdkversion);
    printf("Livox SDK version %d.%d.%d\n", _sdkversion.major, _sdkversion.minor, _sdkversion.patch);
    SetBroadcastCallback(LdsLidar::OnDeviceBroadcast);
    SetDeviceStateUpdateCallback(LdsLidar::OnDeviceChange);
    /** Add commandline input broadcast code */
    for (const auto &input_str : broadcast_code_strs)
    {
        LdsLidar::AddBroadcastCodeToWhitelist(input_str.c_str());
    }
    /** Add local broadcast code */
    LdsLidar::AddLocalBroadcastCode();
    if (whitelist_count_)
    {
        LdsLidar::DisableAutoConnectMode();
        printf("Disable auto connect mode!\n");

        printf("List all broadcast code in whiltelist:\n");
        for (uint32_t i = 0; i < whitelist_count_; i++)
        {
            printf("%s\n", broadcast_code_whitelist_[i]);
        }
    }
    else
    {
        LdsLidar::EnableAutoConnectMode();
        printf("No broadcast code was added to whitelist, swith to automatic connection mode!\n");
    }
    /** Start livox sdk to receive lidar data */
    if (!Start())
    {
        Uninit();
        printf("Livox-SDK init fail!\n");
        return -1;
    }
    /** Add here, only for callback use */
    if (g_lidars == nullptr)
    {
        g_lidars = this;
    }
    is_initialized_ = true;
    printf("Livox-SDK init success!\n");
    return 0;
}

int LdsLidar::DeInitLdsLidar(void)
{
    if (!is_initialized_)
    {
        printf("LiDAR data source is not exit");
        return -1;
    }
    Uninit();
    printf("Livox SDK Deinit completely!\n");
    return 0;
}

void LdsLidar::LivoxImudata(LivoxImuPoint *p_point_data, uint32_t data_num, std::shared_ptr<Eigen::Vector3d> &gyro, std::shared_ptr<Eigen::Vector3d> &acc)
{
    gyro.reset(new Eigen::Vector3d(p_point_data->gyro_x, p_point_data->gyro_y, p_point_data->gyro_z));
    acc.reset(new Eigen::Vector3d(p_point_data->acc_x, p_point_data->acc_y, p_point_data->acc_z));
}

/*
 * woshishabi
 */
/** Static function in LdsLidar for callback or event process ------------------------------------*/
/** Receiving point cloud data from Livox LiDAR. */
void LdsLidar::GetLidarDataCb(uint8_t handle, LivoxEthPacket *data,
                              uint32_t data_num, void *client_data)
{
    static int frameNum = 0, PackageNum = 0;
    static bool FullRoTs = false;
    using namespace std;
    // static int num2 = 0;
    // static double starttime;
    // static timeval time;
    // if (num2 == 0)
    // {
    //     gettimeofday(&time, nullptr);
    //     starttime = time.tv_sec * 1e3 + time.tv_usec * 1e-3;
    // }
    //拷贝内存
    // auto* data_cp = (LivoxEthPacket*)malloc(sizeof(LivoxEthPacket) * data_num);
    // LivoxEthPacket* data_cpy = (LivoxEthPacket*)memcpy(data_cp, data, sizeof(LivoxEthPacket) * data_num);
    static int num = 0;
    // static int num2 = 0;
    // LdsLidar *lidar_this = static_cast<LdsLidar *>(client_data);
    LdsLidar *this_ = static_cast<LdsLidar *>(client_data);
    LivoxEthPacket *eth_packet = data;
    if (!data || !data_num || (handle >= kMaxLidarCount))
    {
        return;
    }
    //获取了原始数据后马上返回。
    // std::cout << "data_num is: " << data_num << std::endl;
    if (eth_packet)
    {
        // static std::vector<LivoxDualExtendRawPoint> tempDatas;
        static std::vector<LivoxExtendRawPoint> tempDatas;
        tempDatas.clear();
        tempDatas.resize(48);
        this_->data_recveive_count_[handle]++;
        // this_->cloud_mu.lock();
        // this_->cloudI_mu.lock();
        // RawClientDatas.push_back(data);
        for (size_t i = 0; i < 48; i++)
        {
            // LivoxDualExtendRawPoint *tdata1 = (LivoxDualExtendRawPoint *)data->data + i;
            LivoxExtendRawPoint *tdata = (LivoxExtendRawPoint *)data->data + i;
            // memcpy(&tdata, data->data+i, sizeof(LivoxRawPoint));
            // tempDatas.push_back(*((LivoxDualExtendRawPoint *)data->data + i));
            tempDatas[i] = *tdata;
            // std::cout << tdata->x1 / 1000. << "  " << tdata->y1 / 1000.<< "  " << tdata->z1 / 1000. << std::endl;
            // LivoxDualExtendRawPoint Temp;
            // Temp.x1 = tdata->x1;
            // Temp.x2 = tdata->x2;
            // Temp.y1 = tdata->y1;
            // Temp.y2 = tdata->y2;
            // Temp.z1 = tdata->z1;
            // Temp.z2 = tdata->z2;
            // Temp.reflectivity1 = tdata->reflectivity1;
            // Temp.reflectivity2 = tdata->reflectivity2;
            // std::cout << "x1, y1, z1: " << Temp.x1 << "  " << Temp.y1 << "  " << Temp.z1 << std::endl;
            // RawDatas.push_back(*tdata);
            // this_->RawDatas[num2 % (500 * 48)] = tdata;
            // num2++;
        }
        // std::cout << "tempDatas: " << &tempDatas[0] << std::endl;
        std::thread GetCloudThreads(ThreadToCloud, frameNum, PackageNum, tempDatas);
        GetCloudThreads.detach();
        PackageNum++;

        //数据包数量等于250，完成一帧的采样
        if (PackageNum == 250)
        {
            Gydata tempRoT;
            tempRoT.Tran = Eigen::Vector3f(0, 0, 0);
            tempRoT.yaw = 0;
            tempRoT.sectime = 0;
            //获取陀螺仪数据
            // myodo.DataCB(tempRoT);
            if (FullRoTs == false)
            {
                this_->RoTs.push_back(tempRoT);
            }
            else
            {
                this_->RoTs.pop_front();
                this_->RoTs.push_back(tempRoT);
            }

            //循环遍历每个线程是否已经完成存图操作
            int i = 0;
            while (true)
            {
                if (this_->Tags[i] == true)
                {
                    if (i == 249)
                        break;

                    i++;
                }
            }
            //将采样得到的点云存到输出帧里
            
            if (FullRoTs == true)
            {
                FrameLock.lock();
                TempAll.pop_front();
                // pcl::copyPointCloud(*this_->Tempall[frameNum], *this_->OutputAll[frameNum]);
                TempAll.push_back(*this_->ChangedCloudI);
            }
            else
            {
                FrameLock.lock();
                TempAll.push_back(*this_->ChangedCloudI);
            }
            FrameLock.unlock();
            pcl::visualization::CloudViewer viewer("viewer Changed");
            viewer.showCloud(this_->ChangedCloudI);
            while(!viewer.wasStopped()){}
            this_->ChangedCloudI->clear();
            // ChangedCloudI->resize(96 * 250);

            //将数据包序号归零
            //将帧序号加一
            PackageNum = 0;
            frameNum++;

            //当帧序号等于5时，则回到第一张（序号0）进行赋值
            if (frameNum == 5)
            {
                frameNum = 0;
                FullRoTs = true; //表示RoTs已经是完成一次完整赋值了
            }
        }

        // if (num2++ > 500)
        if (false)
        {
            // this_->cloud_mu.lock();
            // this_->cloudI_mu.lock();
            for (size_t i = 0; i < 48; i++)
                this_->RawDatas.erase(this_->RawDatas.begin());
            // RawClientDatas.pop_front();
            // RawDatas.pop_front();
            // this_->cloudI_mu.unlock();
            // this_->cloud_mu.unlock();
            // num2--;
        }
        // this_->cloudI_mu.unlock();
        // this_->cloud_mu.unlock();
        // std::cout << "start2" << std::endl;
        // std::cout << RawDatas.front().x1 << "  " << RawDatas.front().x2 << std::endl;
        // std::cout << RawDatas.back().x1 << "  " << RawDatas.back().x2 << std::endl;
        // std::cout << RawDatas.at(RawDatas.size() / 2).x1 << "  " << RawDatas.at(RawDatas.size() / 2).x2 << std::endl;
    }
    // std::cout << "saved step " << std::endl;
    // return;

    // printf("%d\n",data_num);
    // else
    // {
    if (false)
    {
        this_->data_recveive_count_[handle]++;
        this_->cloud_mu.lock();
        this_->cloudI_mu.lock();
        // myodo.DataCB(OriginRoT);
        /** Parsing the timestamp and the point cloud data. */
        uint64_t cur_timestamp = *((uint64_t *)(data->timestamp));
        // printf("%lu\n",cur_timestamp);
        if (data->data_type == kCartesian)
        {
            // DepthImg = cv::Mat::zeros(1024, 1280, CV_8UC1);
            for (size_t i = 0; i < data_num; i++)
            {
                // std::cout << "data_num is: " << data_num << std::endl;
                bool isEmpty = false;
                pcl::PointXYZ point;
                Point tPoint;
                PointI pointI;
                LivoxRawPoint *p_point_data = (LivoxRawPoint *)data->data + i;
                point.x = p_point_data->x / 1000.;
                point.y = p_point_data->y / 1000.;
                point.z = p_point_data->z / 1000.;
                this_->cloud->points[num] = point;
                pointI.x = p_point_data->x / 1000.;
                pointI.y = p_point_data->y / 1000.;
                pointI.z = p_point_data->z / 1000.;
                pointI.intensity = p_point_data->reflectivity;
                this_->Icloud->points[num] = pointI;
                num++;
                num = num % this_->max_cloud_size;
                // if(num == 0)
                //     DepthImg = cv::Mat::zeros(1024, 1280, CV_8UC1);

                // if (point.x == 0 || point.x > 10)
                //     continue;
                // if (abs(point.y) > 6 || abs(point.z) > 1.7)
                //     continue;
                // Eigen::Vector4f PxCoor;
                // Eigen::Vector4f CamCoor;
                // CamCoor = RoT * Eigen::Vector4f(point.x, point.y, point.z, 1);
                //     PxCoor = CamIntrix * CamCoor;
                // float u, v;
                // u = PxCoor(0) / CamCoor(2);
                // v = PxCoor(1) / CamCoor(2);
                // if (u < 0 || u > 1280 || v < 0 || v > 1024)
                //     continue;

                // DepthImg.at<short>(v,u) = CamCoor(2) * 1000;
            }
        }
        else if (data->data_type == kSpherical)
        {
            LivoxSpherPoint *p_point_data = (LivoxSpherPoint *)data->data;
        }
        else if (data->data_type == kExtendCartesian)
        {
            LivoxExtendRawPoint *p_point_data = (LivoxExtendRawPoint *)data->data;
        }
        else if (data->data_type == kExtendSpherical)
        {
            LivoxExtendSpherPoint *p_point_data = (LivoxExtendSpherPoint *)data->data;
        }
        else if (data->data_type == kDualExtendCartesian)
        {
            for (size_t i = 0; i < data_num; i++)
            {
                pcl::PointXYZ point;
                PointI pointI;
                LivoxDualExtendRawPoint *p_point_data = (LivoxDualExtendRawPoint *)data->data + i;
                //第一回波
                point.x = p_point_data->x1 / 1000.;
                point.y = p_point_data->y1 / 1000.;
                point.z = p_point_data->z1 / 1000.;
                this_->cloud->points[num] = point;

                pointI.x = p_point_data->x1 / 1000.;
                pointI.y = p_point_data->y1 / 1000.;
                pointI.z = p_point_data->z1 / 1000.;
                pointI.intensity = p_point_data->reflectivity1;
                this_->Icloud->points[num] = pointI;
                num++;
                num = num % this_->max_cloud_size;
                //第二回波
                point.x = p_point_data->x2 / 1000.;
                point.y = p_point_data->y2 / 1000.;
                point.z = p_point_data->z2 / 1000.;
                this_->cloud->points[num] = point;

                pointI.x = p_point_data->x2 / 1000.;
                pointI.y = p_point_data->y2 / 1000.;
                pointI.z = p_point_data->z2 / 1000.;
                pointI.intensity = p_point_data->reflectivity2;
                this_->Icloud->points[num] = pointI;
                num++;
                num = num % this_->max_cloud_size;
            }
        }
        else if (data->data_type == kDualExtendSpherical)
        {
            LivoxDualExtendSpherPoint *p_point_data = (LivoxDualExtendSpherPoint *)data->data;
        }
        else if (data->data_type == kImu)
        {
            LivoxImuPoint *p_point_data = (LivoxImuPoint *)data->data;
        }
        else if (data->data_type == kTripleExtendCartesian)
        {
            LivoxTripleExtendRawPoint *p_point_data = (LivoxTripleExtendRawPoint *)data->data;
        }
        else if (data->data_type == kTripleExtendSpherical)
        {
            LivoxTripleExtendSpherPoint *p_point_data = (LivoxTripleExtendSpherPoint *)data->data;
        } // else
        //        {
        //            printf("---\n");
        //        }
        else if (data->data_type = kImu)
        {
            LivoxImuPoint *p_point_data = (LivoxImuPoint *)data->data;
            std::shared_ptr<Eigen::Vector3d> gyro_tp, acc_tp;
            LivoxImudata(p_point_data, data_num, gyro_tp, acc_tp);
            if (gyro_tp && acc_tp)
            {
                imu_mu.lock();
                mgyro = gyro_tp;
                macc = acc_tp;
                imu_mu.unlock();
            }
        }
        this_->cloudI_mu.unlock();
        this_->cloud_mu.unlock();
        //   printf("there are %d packets of zero data of %d data\n",zero_num,data_num);
        // }
        // free(data_cp);
    }
}

void LdsLidar::getImuData(std::shared_ptr<Eigen::Vector3d> &gyro, std::shared_ptr<Eigen::Vector3d> &acc, bool destroy)
{
    if (destroy)
    {
        imu_mu.lock();
        auto temp1 = mgyro;
        auto temp2 = macc;
        mgyro.reset();
        macc.reset();
        gyro = temp1;
        acc = temp2;
    }
    else
    {
        imu_mu.lock();
        gyro = mgyro;
        acc = macc;
    }
    imu_mu.unlock();
}

void LdsLidar::OnDeviceBroadcast(const BroadcastDeviceInfo *info)
{
    if (info == nullptr)
    {
        return;
    }
    if (info->dev_type == kDeviceTypeHub)
    {
        printf("In lidar mode, couldn't connect a hub : %s\n", info->broadcast_code);
        return;
    }
    if (g_lidars->IsAutoConnectMode())
    {
        printf("In automatic connection mode, will connect %s\n", info->broadcast_code);
    }
    else
    {
        if (!g_lidars->FindInWhitelist(info->broadcast_code))
        {
            printf("Not in the whitelist, please add %s to if want to connect!\n",
                   info->broadcast_code);
            return;
        }
    }
    livox_status result = kStatusFailure;
    uint8_t handle = 0;
    result = AddLidarToConnect(info->broadcast_code, &handle);
    if (result == kStatusSuccess && handle < kMaxLidarCount)
    {
        LidarDevice *p_lidar = &(g_lidars->lidars_[handle]);
        p_lidar->handle = handle;
        p_lidar->connect_state = kConnectStateOff;
        p_lidar->config.enable_fan = true;
        p_lidar->config.return_mode = kStrongestReturn;
        p_lidar->config.coordinate = kCoordinateCartesian;
        p_lidar->config.imu_rate = kImuFreq200Hz;
        SetDataCallback(handle, LdsLidar::GetLidarDataCb, (void *)g_lidars);
    }
    else
    {
        printf("Add lidar to connect is failed : %d %d \n", result, handle);
    }
}

/** Callback function of changing of device state. */
void LdsLidar::OnDeviceChange(const DeviceInfo *info, DeviceEvent type)
{
    if (info == nullptr)
    {
        return;
    }
    uint8_t handle = info->handle;
    if (handle >= kMaxLidarCount)
    {
        return;
    }
    LidarDevice *p_lidar = &(g_lidars->lidars_[handle]);
    if (type == kEventConnect)
    {
        QueryDeviceInformation(handle, DeviceInformationCb, g_lidars);
        if (p_lidar->connect_state == kConnectStateOff)
        {
            p_lidar->connect_state = kConnectStateOn;
            p_lidar->info = *info;
        }
        printf("[WARNING] Lidar sn: [%s] Connect!!!\n", info->broadcast_code);
    }
    else if (type == kEventDisconnect)
    {
        p_lidar->connect_state = kConnectStateOff;
        printf("[WARNING] Lidar sn: [%s] Disconnect!!!\n", info->broadcast_code);
    }
    else if (type == kEventStateChange)
    {
        p_lidar->info = *info;
        printf("[WARNING] Lidar sn: [%s] StateChange!!!\n", info->broadcast_code);
    }
    if (p_lidar->connect_state == kConnectStateOn)
    {
        printf("Device Working State %d\n", p_lidar->info.state);
        if (p_lidar->info.state == kLidarStateInit)
        {
            printf("Device State Change Progress %u\n", p_lidar->info.status.progress);
        }
        else
        {
            printf("Device State Error Code 0X%08x\n", p_lidar->info.status.status_code.error_code);
        }
        printf("Device feature %d\n", p_lidar->info.feature);
        SetErrorMessageCallback(handle, LdsLidar::LidarErrorStatusCb);
        /** Config lidar parameter */
        if (p_lidar->info.state == kLidarStateNormal)
        {
            if (p_lidar->config.coordinate != 0)
            {
                SetSphericalCoordinate(handle, LdsLidar::SetCoordinateCb, g_lidars);
            }
            else
            {
                SetCartesianCoordinate(handle, LdsLidar::SetCoordinateCb, g_lidars);
            }
            p_lidar->config.set_bits |= kConfigCoordinate;
            if (kDeviceTypeLidarMid40 != info->type)
            {
                LidarSetPointCloudReturnMode(handle, (PointCloudReturnMode)(p_lidar->config.return_mode),
                                             LdsLidar::SetPointCloudReturnModeCb, g_lidars);
                p_lidar->config.set_bits |= kConfigReturnMode;
            }
            if (kDeviceTypeLidarMid40 != info->type && kDeviceTypeLidarMid70 != info->type)
            {
                LidarSetImuPushFrequency(handle, (ImuFreq)(p_lidar->config.imu_rate),
                                         LdsLidar::SetImuRatePushFrequencyCb, g_lidars);
                p_lidar->config.set_bits |= kConfigImuRate;
            }
            p_lidar->connect_state = kConnectStateConfig;
        }
    }
}

/** Query the firmware version of Livox LiDAR. */
void LdsLidar::DeviceInformationCb(livox_status status, uint8_t handle,
                                   DeviceInformationResponse *ack, void *client_data)
{
    if (status != kStatusSuccess)
    {
        printf("Device Query Informations Failed : %d\n", status);
    }
    if (ack)
    {
        printf("firm ver: %d.%d.%d.%d\n",
               ack->firmware_version[0],
               ack->firmware_version[1],
               ack->firmware_version[2],
               ack->firmware_version[3]);
    }
}

/** Callback function of Lidar error message. */
void LdsLidar::LidarErrorStatusCb(livox_status status, uint8_t handle, ErrorMessage *message)
{
    static uint32_t error_message_count = 0;
    if (message != nullptr)
    {
        ++error_message_count;
        if (0 == (error_message_count % 100))
        {
            printf("handle: %u\n", handle);
            printf("temp_status : %u\n", message->lidar_error_code.temp_status);
            printf("volt_status : %u\n", message->lidar_error_code.volt_status);
            printf("motor_status : %u\n", message->lidar_error_code.motor_status);
            printf("dirty_warn : %u\n", message->lidar_error_code.dirty_warn);
            printf("firmware_err : %u\n", message->lidar_error_code.firmware_err);
            printf("pps_status : %u\n", message->lidar_error_code.device_status);
            printf("fan_status : %u\n", message->lidar_error_code.fan_status);
            printf("self_heating : %u\n", message->lidar_error_code.self_heating);
            printf("ptp_status : %u\n", message->lidar_error_code.ptp_status);
            printf("time_sync_status : %u\n", message->lidar_error_code.time_sync_status);
            printf("system_status : %u\n", message->lidar_error_code.system_status);
        }
    }
}

void LdsLidar::ControlFanCb(livox_status status, uint8_t handle,
                            uint8_t response, void *client_data)
{
}

void LdsLidar::SetPointCloudReturnModeCb(livox_status status, uint8_t handle,
                                         uint8_t response, void *client_data)
{
    LdsLidar *lds_lidar = static_cast<LdsLidar *>(client_data);

    if (handle >= kMaxLidarCount)
    {
        return;
    }
    LidarDevice *p_lidar = &(lds_lidar->lidars_[handle]);
    if (status == kStatusSuccess)
    {
        p_lidar->config.set_bits &= ~((uint32_t)(kConfigReturnMode));
        printf("Set return mode success!\n");
        if (!p_lidar->config.set_bits)
        {
            LidarStartSampling(handle, LdsLidar::StartSampleCb, lds_lidar);
            p_lidar->connect_state = kConnectStateSampling;
        }
    }
    else
    {
        LidarSetPointCloudReturnMode(handle, (PointCloudReturnMode)(p_lidar->config.return_mode),
                                     LdsLidar::SetPointCloudReturnModeCb, lds_lidar);
        printf("Set return mode fail, try again!\n");
    }
}

void LdsLidar::SetCoordinateCb(livox_status status, uint8_t handle,
                               uint8_t response, void *client_data)
{
    LdsLidar *lds_lidar = static_cast<LdsLidar *>(client_data);
    if (handle >= kMaxLidarCount)
    {
        return;
    }
    LidarDevice *p_lidar = &(lds_lidar->lidars_[handle]);
    if (status == kStatusSuccess)
    {
        p_lidar->config.set_bits &= ~((uint32_t)(kConfigCoordinate));
        printf("Set coordinate success!\n");

        if (!p_lidar->config.set_bits)
        {
            LidarStartSampling(handle, LdsLidar::StartSampleCb, lds_lidar);
            p_lidar->connect_state = kConnectStateSampling;
        }
    }
    else
    {
        if (p_lidar->config.coordinate != 0)
        {
            SetSphericalCoordinate(handle, LdsLidar::SetCoordinateCb, lds_lidar);
        }
        else
        {
            SetCartesianCoordinate(handle, LdsLidar::SetCoordinateCb, lds_lidar);
        }

        printf("Set coordinate fail, try again!\n");
    }
}

void LdsLidar::SetImuRatePushFrequencyCb(livox_status status, uint8_t handle,
                                         uint8_t response, void *client_data)
{
    LdsLidar *lds_lidar = static_cast<LdsLidar *>(client_data);

    if (handle >= kMaxLidarCount)
    {
        return;
    }
    LidarDevice *p_lidar = &(lds_lidar->lidars_[handle]);

    if (status == kStatusSuccess)
    {
        p_lidar->config.set_bits &= ~((uint32_t)(kConfigImuRate));
        printf("Set imu rate success!\n");

        if (!p_lidar->config.set_bits)
        {
            LidarStartSampling(handle, LdsLidar::StartSampleCb, lds_lidar);
            p_lidar->connect_state = kConnectStateSampling;
        }
    }
    else
    {
        LidarSetImuPushFrequency(handle, (ImuFreq)(p_lidar->config.imu_rate),
                                 LdsLidar::SetImuRatePushFrequencyCb, lds_lidar);
        printf("Set imu rate fail, try again!\n");
    }
}

/** Callback function of starting sampling. */
void LdsLidar::StartSampleCb(livox_status status, uint8_t handle,
                             uint8_t response, void *client_data)
{
    LdsLidar *lds_lidar = static_cast<LdsLidar *>(client_data);

    if (handle >= kMaxLidarCount)
    {
        return;
    }

    LidarDevice *p_lidar = &(lds_lidar->lidars_[handle]);
    if (status == kStatusSuccess)
    {
        if (response != 0)
        {
            p_lidar->connect_state = kConnectStateOn;
            printf("Lidar start sample fail : state[%d] handle[%d] res[%d]\n",
                   status, handle, response);
        }
        else
        {
            printf("Lidar start sample success\n");
        }
    }
    else if (status == kStatusTimeout)
    {
        p_lidar->connect_state = kConnectStateOn;
        printf("Lidar start sample timeout : state[%d] handle[%d] res[%d]\n",
               status, handle, response);
    }
}

/** Callback function of stopping sampling. */
void LdsLidar::StopSampleCb(livox_status status, uint8_t handle,
                            uint8_t response, void *client_data)
{
}

/** Add broadcast code to whitelist */
int LdsLidar::AddBroadcastCodeToWhitelist(const char *bd_code)
{
    if (!bd_code || (strlen(bd_code) > kBroadcastCodeSize) ||
        (whitelist_count_ >= kMaxLidarCount))
    {
        return -1;
    }

    if (LdsLidar::FindInWhitelist(bd_code))
    {
        printf("%s is alrealy exist!\n", bd_code);
        return -1;
    }

    strcpy(broadcast_code_whitelist_[whitelist_count_], bd_code);
    ++whitelist_count_;

    return 0;
}

void LdsLidar::AddLocalBroadcastCode(void)
{
    for (size_t i = 0; i < sizeof(local_broadcast_code_list) / sizeof(intptr_t); ++i)
    {
        std::string invalid_bd = "000000000";
        printf("Local broadcast code : %s\n", local_broadcast_code_list[i]);
        if ((kBroadcastCodeSize == strlen(local_broadcast_code_list[i]) + 1) &&
            (nullptr == strstr(local_broadcast_code_list[i], invalid_bd.c_str())))
        {
            LdsLidar::AddBroadcastCodeToWhitelist(local_broadcast_code_list[i]);
        }
        else
        {
            printf("Invalid local broadcast code : %s\n", local_broadcast_code_list[i]);
        }
    }
}

bool LdsLidar::FindInWhitelist(const char *bd_code)
{
    if (!bd_code)
    {
        return false;
    }

    for (uint32_t i = 0; i < whitelist_count_; i++)
    {
        if (strncmp(bd_code, broadcast_code_whitelist_[i], kBroadcastCodeSize) == 0)
        {
            return true;
        }
    }

    return false;
}

void LdsLidar::getcloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out)
{
    cloud_mu.lock();
    pcl::copyPointCloud(*cloud, *cloud_out);
    cloud_mu.unlock();
}

void LdsLidar::getcloud(PcloudI cloud_out)
{
    cloudI_mu.lock();
    // RoT_mu.lock();
    pcl::copyPointCloud(*Icloud, *cloud_out);
    // outputRoT = OriginRoT;
    cloudI_mu.unlock();
}

void LdsLidar::Getcloud(PcloudI &cloud_out, int num)
{
    PcloudI temp(new CloudI());
    cloudI_mu.lock();
    // temp->points.resize(2 * RawDatas.size());
    temp->points.resize(RawDatas.size());

    for (size_t i = 0; i < RawDatas.size(); i++)
    {
        // for (size_t j = 0; j < 48; j++)
        // {

        bool isEmpty = false;
        // Point point;
        PointI pointI1, pointI2;
        // LivoxRawPoint *p_point_data = (LivoxRawPoint *)(RawClientDatas.at(i)->data) + j;
        // LivoxRawPoint *p_point_data = RawDatas.at(i);
        LivoxDualExtendRawPoint p_point_data = RawDatas.at(i);

        // point.x = p_point_data.x1 / 1000.;
        // point.y = p_point_data.y1 / 1000.;
        // point.z = p_point_data.z1 / 1000.;
        // this_->cloud->points[num] = point;

        // point.x = p_point_data.x2 / 1000.;
        // point.y = p_point_data.y2 / 1000.;
        // point.z = p_point_data.z2 / 1000.;
        // this_->cloud->points[num] = point;
        pointI1.x = p_point_data.x1 / 1000.;
        pointI1.y = p_point_data.y1 / 1000.;
        pointI1.z = p_point_data.z1 / 1000.;
        pointI1.intensity = p_point_data.reflectivity1;

        pointI2.x = p_point_data.x2 / 1000.;
        pointI2.y = p_point_data.y2 / 1000.;
        pointI2.z = p_point_data.z2 / 1000.;

        // pointI.intensity = p_point_data->reflectivity;
        pointI2.intensity = p_point_data.reflectivity2;
        // this_->Icloud->points[num] = pointI1;
        // std::cout << "x1, y1, z1: " << pointI1.x << "  " << pointI1.y << "  " <<pointI1.z << std::endl;
        // cloud_out->points.push_back(pointI1);
        // cloud_out->points.emplace_back(pointI1);
        // cloud_out->points.emplace_back(pointI2);
        size_t n = 2 * i;
        temp->points[i] = pointI1;
        // temp->points[n+1] = pointI2;
        // num++;
        // num = num % this_->max_cloud_size;
        // }
    }
    pcl::copyPointCloud(*temp, *cloud_out);
    cloudI_mu.unlock();
}

void LdsLidar::clearcloud()
{
    cloud_mu.lock();
    cloud->points.resize(max_cloud_size, pcl::PointXYZ(0, 0, 0));
    cloud_mu.unlock();
}

void LdsLidar::getall(PcloudI &cloud_out, cv::Mat &InputImg)
{
    cloud_mu.lock();
    pcl::copyPointCloud(*Icloud, *cloud_out);
    Eigen::Vector4f CamCoor;
    Eigen::Vector3f PxCoor;
    for (size_t i = 0; i < Icloud->size(); i++)
    {
        PointI tPoint = Icloud->points[i];
        float u, v;
        if (tPoint.x == 0 || tPoint.x > 10)
            continue;
        // if(abs(tPoint.y) > 6 || tPoint.z < -0.5 || tPoint.z > 1.7)
        //     continue;

        // LdCoor << tPoint.x, tPoint.y, tPoint.z, 1;
        CamCoor = RoT * Eigen::Vector4f(tPoint.x, tPoint.y, tPoint.z, 1);
        PxCoor = CamIntrix * Eigen::Vector3f(CamCoor.x(), CamCoor.y(), CamCoor.z());
        u = PxCoor(0) / CamCoor(2);
        v = PxCoor(1) / CamCoor(2);
        if (u < 0 || u > 1280 || v < 0 || v > 1024)
            continue;
        InputImg.at<short>(v, u) = CamCoor(2) * 1000;
    }
    cloud_mu.unlock();
}

void LdsLidar::ThreadToCloud(int frameNum, int num, std::vector<LivoxExtendRawPoint> data)
{
    // std::cout << "data: " << &data[0] << std::endl;
    // sleep(2);
    PointI pointI1, pointI2;
    for (size_t i = 0; i < data.size(); i++)
    {
        pointI1.x = data[i].x;
        pointI1.y = data[i].y;
        pointI1.z = data[i].z;
        pointI1.intensity = data[i].reflectivity;
        // pointI1.x = data[i].x1;
        // pointI1.y = data[i].y1;
        // pointI1.z = data[i].z1;
        // pointI1.intensity = data[i].reflectivity1;
        // pointI2.x = data[i].x2;
        // pointI2.y = data[i].y2;
        // pointI2.z = data[i].z2;
        // pointI2.intensity = data[i].reflectivity2;
        // Tempall[frameNum]->points[96 * num + 2 * i] = pointI1;
        // Tempall[frameNum]->points[96 * num + 2 * i + 1] = pointI2;
        // ChangedCloudI->points[96 * num + 2 * i] = pointI1;
        // ChangedCloudI->points[96 * num + 2 * i + 1] = pointI2;
        change_mu.lock();
        ChangedCloudI->points.push_back(pointI1);
        // ChangedCloudI->points.push_back(pointI2);
        change_mu.unlock();
    }
    
    //表示该次数据包已完成转换
    Tags[num] = true;
}

void LdsLidar::Get5Cloud(std::vector<CloudI> &Clouds, std::vector<Gydata> &Gydatas)
{
    while (TempAll.size() != 5)
    {
        sleep(1);
    }

    FrameLock.lock();
    for (size_t i = 0; i < 5; i++)
    {
        if (!TempAll[i].points.empty())
        {
            TempAll[i].is_dense = false;
            PcloudI temp(new CloudI());
            // std::cout << TempAll[i].points.size() << std::endl;
            // pcl::copyPointCloud(TempAll[i], *Clouds[i]);
            pcl::copyPointCloud(TempAll[i], Clouds[i]);
            // Clouds[i].swap(*temp);
            Gydatas[i] = RoTs[i];
        }
    }
    FrameLock.unlock();
}